/*
*****************************************************************
* Copyright All Reserved © 2015-2025 Solonix-Chu
*****************************************************************
*/

#define LOG_TAG "uorb.core"
#define LOG_LVL LOG_LVL_INFO

#include "uORB.h"
#include "uorb_node.h"
#include "uorb_utils.h"
#include <rtthread.h>


/* -------------------------------------- */
/* Topic utility                           */
/* -------------------------------------- */

int orb_exists(const struct orb_metadata_s *meta, int instance)
{
    return orb_node_exists(meta, instance) ? RT_EOK : -RT_ERROR;
}

int orb_group_count(const struct orb_metadata_s *meta)
{
    int count = 0;
    for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++)
    {
        if (orb_node_exists(meta, i))
        {
            count++;
        }
    }
    return count;
}


/* -------------------------------------- */
/* Advertise wrappers                      */
/* -------------------------------------- */

orb_advert_t orb_advertise_multi_queue(const struct orb_metadata_s *meta, const void *data, int *instance, unsigned int queue_size)
{
    if (!meta)
    {
        return RT_NULL;
    }

    orb_node_t *node = RT_NULL;

    // 允许的最大instance个数

    int max_inst = ORB_MULTI_MAX_INSTANCES;
    int inst     = 0;

    int def_inst = get_default_instances(meta);

    if (!instance)
    {
        /* 未给出instance输出指针：按默认实例上限（若未配置则为1）搜索可用实例 */
        max_inst = (def_inst > 0) ? def_inst : 1;
    }
    else
    {
        /* 允许在提供instance指针时遍历全部可用实例上限 */
        max_inst = ORB_MULTI_MAX_INSTANCES;
    }

    int selected_inst = -1;

    // 搜索实例是否存在，不存在则创建，存在则判断是否公告
    for (inst = 0; inst < max_inst; inst++)
    {
        node = orb_node_find(meta, inst);

        if (node)
        {
            if (!node->advertised)
            {
                selected_inst = inst;
                break;
            }
            else
            {
                // 已经公告，占用该实例，继续查找
                node = RT_NULL;
                continue;
            }
        }
        else
        {
            /* 当调用方未指定队列长度(传入0)时，orb_node_create 将使用默认队列长度（若有） */
            node = orb_node_create(meta, inst, (rt_uint8_t)queue_size);
            if (!node)
            {
                // 创建失败，继续尝试下一个实例
                continue;
            }
            selected_inst = inst;
            break;
        }
    }

    // 未找到可用实例
    if (selected_inst < 0 || !node)
    {
        return RT_NULL;
    }

    // 标记为已经公告，只有公告过的主题才能copy和publish数据
    node->advertised = true;
    if (data)
    {
        orb_node_write(node, data);
    }
    // 返回inst
    if (instance)
    {
        *instance = selected_inst;
    }
#ifdef UORB_REGISTER_AS_DEVICE
    rt_uorb_register_topic(meta, (rt_uint8_t)selected_inst);
#endif

    return node;
}


orb_advert_t orb_advertise(const struct orb_metadata_s *meta, const void *data)
{
    return orb_advertise_multi_queue(meta, data, RT_NULL, 1);
}

orb_advert_t orb_advertise_queue(const struct orb_metadata_s *meta, const void *data, unsigned int queue_size)
{
    return orb_advertise_multi_queue(meta, data, RT_NULL, queue_size);
}

orb_advert_t orb_advertise_multi(const struct orb_metadata_s *meta, const void *data, int *instance)
{
    return orb_advertise_multi_queue(meta, data, instance, 1);
}


int orb_unadvertise(orb_node_t *node)
{
    if (!node)
    {
        return -RT_ERROR;
    }

    node->advertised = false;

    /* 若无人订阅则释放节点（与设备化策略配合：设备注销也会尝试回收） */
    if (node->subscriber_count == 0)
    {
        return orb_node_delete(node);
    }

    return RT_EOK;
}

int orb_publish(const struct orb_metadata_s *meta, orb_node_t *node, const void *data)
{
    if (!data)
    {
        return -RT_EINVAL;
    }

    if (!meta && !node)
    {
        return -RT_EINVAL;
    }
    else if (meta && !node)
    {
        /* 优先选择已公告的最小实例，若不存在则回退实例0 */
        for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++)
        {
            orb_node_t *candidate = orb_node_find(meta, i);
            if (candidate && candidate->advertised)
            {
                node = candidate;
                break;
            }
        }
        if (!node)
        {
            node = orb_node_find(meta, 0);
        }
        if (!node)
        {
            return -RT_ENOENT;
        }
    }
    else if (meta && node)
    {
        if (node->meta != meta)
        {
            return -RT_EINVAL;
        }
    }
    // else (!meta && node) 直接用node

    // 新增：检查节点是否已公告
    if (!node->advertised)
    {
        return -RT_EINVAL;
    }

    if (orb_node_write(node, data) == node->meta->o_size)
    {
        return RT_EOK;
    }

    return -RT_ERROR;
}

int orb_register_callback(const struct orb_metadata_s *meta, uint8_t instance, void (*fn)(void))
{
    if (!meta || !fn)
    {
        return -RT_ERROR;
    }
    orb_node_t *node = orb_node_find(meta, instance);
    if (!node)
    {
        return -RT_ERROR;
    }
    orb_callback_t *item = (orb_callback_t *)rt_calloc(1, sizeof(orb_callback_t));
    if (!item)
    {
        return -RT_ERROR;
    }
    item->call = fn;
    rt_list_insert_after(&node->callbacks, &item->list);
    return RT_EOK;
}

int orb_unregister_callback(const struct orb_metadata_s *meta, uint8_t instance, void (*fn)(void))
{
    if (!meta || !fn)
    {
        return -RT_ERROR;
    }
    orb_node_t *node = orb_node_find(meta, instance);
    if (!node)
    {
        return -RT_ERROR;
    }
    rt_list_t *pos, *tmp;
    rt_list_for_each_safe(pos, tmp, &node->callbacks)
    {
        orb_callback_t *item = rt_list_entry(pos, orb_callback_t, list);
        if (item->call == fn)
        {
            rt_list_remove(&item->list);
            rt_free(item);
            return RT_EOK;
        }
    }
    return -RT_ERROR;
}


/* -------------------------------------- */
/* Subscribe wrappers                      */
/* -------------------------------------- */

orb_subscribe_t *orb_subscribe_multi(const struct orb_metadata_s *meta, uint8_t instance)
{
    orb_subscribe_t *sub = rt_calloc(sizeof(orb_subscribe_t), 1);

    sub->meta       = meta;
    sub->instance   = instance;
    sub->interval   = 0;
    sub->generation = 0;
    sub->node       = orb_node_find(meta, instance);

    // 如果找到了节点，增加订阅者计数并初始化generation
    if (sub->node)
    {
        sub->node->subscriber_count++;
        sub->generation = sub->node->generation;
    }

    return sub;
}

orb_subscribe_t *orb_subscribe(const struct orb_metadata_s *meta)
{
    return orb_subscribe_multi(meta, 0);
}

int orb_unsubscribe(orb_subscribe_t *handle)
{
    if (!handle)
    {
        return -RT_EINVAL;
    }

    if (handle->node)
    {
        handle->node->subscriber_count--;
        /* 若已标记延迟删除且无订阅者且未公告，则回收节点 */
        if (handle->node->subscriber_count == 0 && (handle->node->pending_delete || !handle->node->advertised))
        {
            orb_node_delete(handle->node);
        }
    }

    /* 清理订阅者状态，避免悬挂（即便随后释放） */
    handle->callback_registered   = RT_FALSE;
    handle->last_update_timestamp = 0;
    handle->node                  = RT_NULL;
    handle->generation            = 0;

    rt_free(handle);
    return RT_EOK;
}

//  检查订阅者是否有新数据可读，并支持定时检查
int orb_check(orb_subscribe_t *handle, rt_bool_t *updated)
{
    if (!handle || !updated)
    {
        return -RT_EINVAL;
    }

    if (!orb_node_ready(handle))
    {
        return -RT_ERROR;
    }

    // 简化逻辑：检查generation是否不同来确定是否有更新
    if (handle->interval == 0 || (rt_tick_get() - handle->last_update_timestamp) * 1000 / RT_TICK_PER_SECOND >= handle->interval)
    {
        *updated = handle->generation != handle->node->generation;
        if (*updated)
        {
            /* 推进 generation，使得一次检查消费一次更新信号 */
            handle->generation = handle->node->generation;
        }
        return RT_EOK;
    }
    else
    {
        *updated = RT_FALSE;
        return RT_EOK;
    }
}

int orb_copy(const struct orb_metadata_s *meta, orb_subscribe_t *handle, void *buffer)
{
    if (!meta || !handle || !buffer)
        return -RT_EINVAL;

    // 检查节点是否ready
    if (!orb_node_ready(handle))
        return -RT_ERROR;

    // 读取数据并更新订阅者generation
    int ret = orb_node_read(handle->node, buffer, &handle->generation);
    if (ret > 0)
    {
        // 更新时间戳
        handle->last_update_timestamp = rt_tick_get();
        return ret;
    }
    return ret; // 返回实际的错误码或0
}


/* -------------------------------------- */
/* Interval control                        */
/* -------------------------------------- */

int orb_set_interval(orb_subscribe_t *sub, unsigned interval)
{
    if (!sub)
    {
        return -RT_EINVAL;
    }
    sub->interval = interval;
    return RT_EOK;
}

int orb_get_interval(orb_subscribe_t *sub, unsigned *interval)
{
    if (!sub || !interval)
    {
        return -RT_EINVAL;
    }
    *interval = sub->interval;
    return RT_EOK;
}

int orb_wait(orb_subscribe_t *handle, int timeout_ms)
{
    /* 事件化实现：优先事件等待，其次轮询退避 */
    if (!handle)
    {
        return -RT_EINVAL;
    }
    /* 若尚未就绪则尝试绑定节点 */
    if (!orb_node_ready(handle))
    {
        return -RT_ENOENT;
    }
    /* 事件前先快速检查一次，避免订阅后因旧事件误唤醒 */
    rt_bool_t updated = RT_FALSE;
    if (orb_check(handle, &updated) == RT_EOK && updated)
    {
        return RT_EOK;
    }

    if (handle->node && handle->node->notifier.event)
    {
        int waited = 0;
        int step   = 10;
        while (1)
        {
            int slice = step;
            if (timeout_ms >= 0)
            {
                int remain = timeout_ms - waited;
                if (remain <= 0) return -RT_ETIMEOUT;
                if (remain < step) slice = remain;
            }
            (void)uorb_notifier_wait(&handle->node->notifier, slice);
            (void)orb_check(handle, &updated);
            if (updated)
            {
                return RT_EOK;
            }
            if (timeout_ms >= 0)
            {
                waited += slice;
            }
        }
    }
    /* 退化为轮询 */
    updated    = RT_FALSE;
    int waited = 0;
    int step   = 5;
    while (1)
    {
        if (orb_check(handle, &updated) != RT_EOK)
        {
            return -RT_ERROR;
        }
        if (updated)
        {
            return RT_EOK;
        }
        if (timeout_ms >= 0 && waited >= timeout_ms)
        {
            return -RT_ETIMEOUT;
        }
        rt_thread_mdelay(step);
        waited += step;
    }
}
